#include <cmath>
#include "mathbox.h"

MathBox::MathBox(Water* water, Ship* ship)
: water(water)
, ship(ship)
{
	m11 = 2.0 * ship->mass / (RHO * ship->length * ship->length * ship->draught);
	m22 = m11 + PI * ship->draught / water->waveFrequency / ship->length;
	m66 = 2.0 * ship->Jz / (RHO * water->waveFrequency * ship->length * ship->length * ship->length * ship->draught)
		+ ship->draught * PI / (12.0 * water->waveFrequency * ship->length);
}

MathBox::~MathBox(void)
{
}

void MathBox::Step(double dt)
{
	double ds = ship->velocity * dt / ship->length;
	double shipVelDimless = ship->velocity / water->waveVelocity;
	double shipAngleVelDimless = ship->angleVelocity * ship->length / ship->velocity;

	double k1 = VelocityFunction(shipVelDimless, ship->drift, shipAngleVelDimless, ship->q, ship->coords.course) * ds;
	double m1 = DriftFunction(shipVelDimless, ship->drift, shipAngleVelDimless, ship->q, ship->coords.course) * ds;
	double n1 = AngleVelFunction(shipVelDimless, ship->drift, shipAngleVelDimless, ship->q, ship->coords.course) * ds;
	double o1 = QFunction(shipVelDimless, ship->drift, ship->coords.course) * ds;
	double p1 = CourseFunction(shipAngleVelDimless) * ds;

	double k2 = VelocityFunction(shipVelDimless + k1/2.0, ship->drift + m1/2.0, shipAngleVelDimless + n1/2.0, ship->q + o1/2.0, ship->coords.course + p1/2.0) * ds;
	double m2 = DriftFunction(shipVelDimless + k1/2.0, ship->drift + m1/2.0, shipAngleVelDimless + n1/2.0, ship->q + o1/2.0, ship->coords.course + p1/2.0) * ds;
	double n2 = AngleVelFunction(shipVelDimless + k1/2.0, ship->drift + m1/2.0, shipAngleVelDimless + n1/2.0, ship->q + o1/2.0, ship->coords.course + p1/2.0) * ds;
	double o2 = QFunction(shipVelDimless + k1/2.0, ship->drift + m1/2.0, ship->coords.course + p1/2.0) * ds;
	double p2 = CourseFunction(shipAngleVelDimless + n1/2.0) * ds;

	double k3 = VelocityFunction(shipVelDimless + k2/2.0, ship->drift + m2/2.0, shipAngleVelDimless + n2/2.0, ship->q + o2/2.0, ship->coords.course + p2/2.0) * ds;
	double m3 = DriftFunction(shipVelDimless + k2/2.0, ship->drift + m2/2.0, shipAngleVelDimless + n2/2.0, ship->q + o2/2.0, ship->coords.course + p2/2.0) * ds;
	double n3 = AngleVelFunction(shipVelDimless + k2/2.0, ship->drift + m2/2.0, shipAngleVelDimless + n2/2.0, ship->q + o2/2.0, ship->coords.course + p2/2.0) * ds;
	double o3 = QFunction(shipVelDimless + k2/2.0, ship->drift + m2/2.0, ship->coords.course + p2/2.0) * ds;
	double p3 = CourseFunction(shipAngleVelDimless + n2/2.0) * ds;

	double k4 = VelocityFunction(shipVelDimless + k3, ship->drift + m3, shipAngleVelDimless + n3, ship->q + o3, ship->coords.course + p3) * ds;
	double m4 = DriftFunction(shipVelDimless + k3, ship->drift + m3, shipAngleVelDimless + n3, ship->q + o3, ship->coords.course + p3) * ds;
	double n4 = AngleVelFunction(shipVelDimless + k3, ship->drift + m3, shipAngleVelDimless + n3, ship->q + o3, ship->coords.course + p3) * ds;
	double o4 = QFunction(shipVelDimless + k3, ship->drift + m3, ship->coords.course + p3) * ds;
	double p4 = CourseFunction(shipAngleVelDimless + n3) * ds;

	shipVelDimless += (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0;
	ship->drift += (m1 + 2.0 * m2 + 2.0 * m3 + m4) / 6.0;
	shipAngleVelDimless += (n1 + 2.0 * n2 + 2.0 * n3 + n4) / 6.0;
	ship->q += (o1 + 2.0 * o2 + 2.0 * o3 + o4) / 6.0;
	ship->coords.course += (p1 + 2.0 * p2 + 2.0 * p3 + p4) / 6.0;

	ship->velocity = shipVelDimless * water->waveVelocity;
	ship->angleVelocity = shipAngleVelDimless * ship->velocity / ship->length;

	double a = 0.0;

	if (ship->coords.course < -0.3)
		a = -0.1;



	ship->coords.x += ship->velocity * sin(ship->coords.course - a/* - ship->drift*/) * dt;
	ship->coords.y += ship->velocity * cos(ship->coords.course - a/* - ship->drift*/) * dt;
}

double MathBox::ForceX(const double shipVelDimless, const double q)
{
	double ForceDist = 4.0 * ship->width / water->waveFrequency / ship->length * water->waveAngleAmplitude;
	double ForceAero = 0.0;
	return 2.0 * (ship->Xp - ship->R) / (RHO * water->waveVelocity*water->waveVelocity*shipVelDimless*ship->F)
		+ ForceDist * sin(q) / shipVelDimless / shipVelDimless
		+ ForceAero;
}

double MathBox::ForceY(const double shipVelDimless, const double q, const double shipCourse)
{
	double Cyh = 1.0;
	double ForceDist = 4.0 * ship->width / water->waveFrequency
		/ ship->length * water->waveAngleAmplitude * tan(shipCourse)
		+ PI * ship->draught * sin(shipCourse) / ship->width * (1.0 + shipVelDimless * cos(shipCourse) / 2.0);

	double ForceAero = 0.0;

	return Cyh - ForceDist / shipVelDimless / shipVelDimless * sin(q) + ForceAero;
}

double MathBox::TorgueZ(const double shipVelDimless, const double q, const double shipCourse)
{
	double Mzh = 4.0;
	double TorgueDist = PI * ship->width / 2.0 / water->waveFrequency / water->waveLength
		*(PI*ship->draught/ship->width *(1-shipVelDimless*cos(shipCourse))
		+2*ship->draught/PI/ship->width*pow(water->waveLength/ship->length,2)*shipVelDimless/cos(shipCourse))*sin(shipCourse);
	double TorgueAero = 0.0;

	return Mzh + TorgueDist / shipVelDimless / shipVelDimless * cos(q) + TorgueAero;
}

double MathBox::DriftFunction(const double shipVelDimless, const double shipDrift,
							  const double shipAngleVelDimless, const double q, const double shipCourse)
{
	double forceX = ForceX(shipVelDimless, q);
	double forceY = ForceY(shipVelDimless, q, shipCourse);
	return shipAngleVelDimless * (pow(cos(shipDrift),2) * m11 / m22 + pow(sin(shipDrift),2) * m22 / m11)
		- forceY * cos(shipDrift) / m22 - forceX * sin(shipDrift) / m11;
}

double MathBox::VelocityFunction(const double shipVelDimless, const double shipDrift,
								 const double shipAngleVelDimless, const double q, const double shipCourse)
{
	double forceX = ForceX(shipVelDimless, q);

	double driftDer = DriftFunction(shipVelDimless, shipDrift, shipAngleVelDimless, q, shipCourse);

	return shipVelDimless * tan(shipDrift) * driftDer
		+ shipVelDimless * (forceX - m22 * sin(shipDrift) * shipAngleVelDimless) / (m11 * cos(driftDer));
}

double MathBox::AngleVelFunction(const double shipVelDimless, const double shipDrift,
						const double shipAngleVelDimless, const double q, const double shipCourse)
{
	double torgueZ = TorgueZ(shipVelDimless, q, shipCourse);

	double velDer = VelocityFunction(shipVelDimless, shipDrift, shipAngleVelDimless, q, shipCourse);

	return torgueZ /m66 - shipAngleVelDimless/shipVelDimless * velDer;
}

double MathBox::QFunction(const double shipVelDimless, const double shipDrift, const double shipCourse)
{
	return 2*PI / water->waveLength * (1/shipVelDimless - cos(shipCourse - shipDrift));
}

double MathBox::CourseFunction(const double shipAngleVelDimless)
{
	return shipAngleVelDimless;
}